#include "std_msgs/String.h"
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <ros/package.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
// image
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <cv_bridge/cv_bridge.h>
#include <image_transport/image_transport.h>
#include <tf2/convert.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <Eigen/Dense>
#include <opencv2/calib3d/calib3d.hpp>
// pcl
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include "radar_point_xyzdr.h"
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/kdtree/flann.h>
#include "pcl_ros/transforms.h"
// project color
double colmap[50][3] = {{0, 0, 0.5385}, {0, 0, 0.6154}, {0, 0, 0.6923}, {0, 0, 0.7692}, {0, 0, 0.8462}, {0, 0, 0.9231}, {0, 0, 1.0000}, {0, 0.0769, 1.0000}, {0, 0.1538, 1.0000}, {0, 0.2308, 1.0000}, {0, 0.3846, 1.0000}, {0, 0.4615, 1.0000}, {0, 0.5385, 1.0000}, {0, 0.6154, 1.0000}, {0, 0.6923, 1.0000}, {0, 0.7692, 1.0000}, {0, 0.8462, 1.0000}, {0, 0.9231, 1.0000}, {0, 1.0000, 1.0000}, {0.0769, 1.0000, 0.9231}, {0.1538, 1.0000, 0.8462}, {0.2308, 1.0000, 0.7692}, {0.3077, 1.0000, 0.6923}, {0.3846, 1.0000, 0.6154}, {0.4615, 1.0000, 0.5385}, {0.5385, 1.0000, 0.4615}, {0.6154, 1.0000, 0.3846}, {0.6923, 1.0000, 0.3077}, {0.7692, 1.0000, 0.2308}, {0.8462, 1.0000, 0.1538}, {0.9231, 1.0000, 0.0769}, {1.0000, 1.0000, 0}, {1.0000, 0.9231, 0}, {1.0000, 0.8462, 0}, {1.0000, 0.7692, 0}, {1.0000, 0.6923, 0}, {1.0000, 0.6154, 0}, {1.0000, 0.5385, 0}, {1.0000, 0.4615, 0}, {1.0000, 0.3846, 0}, {1.0000, 0.3077, 0}, {1.0000, 0.2308, 0}, {1.0000, 0.1538, 0}, {1.0000, 0.0769, 0}, {1.0000, 0, 0}, {0.9231, 0, 0}, {0.8462, 0, 0}, {0.7692, 0, 0}, {0.6923, 0, 0}};

struct Rot_Trans_Euler
{
    double m1; 
    double m2;
    double m3;
    double x;
    double m4; 
    double m5;
    double m6;
    double y;
    double m7; 
    double m8;
    double m9;
    double z;
} eul_m;

struct Rot_Trans
{
    double e1; // Joint (Rotation and translation) optimization variables
    double e2;
    double e3;
    double x;
    double y;
    double z;
    std::string to_string() const
    {
        return std::string("{") + "e1:" + std::to_string(e1) + ", e2:" + std::to_string(e2) + ", e3:" + std::to_string(e3) + ", x:" + std::to_string(x) + ", y:" + std::to_string(y) + ", z:" + std::to_string(z) + "}";
    }
} eul_t, eul_it;



struct initial_parameters
{
    std::string camera_topic;
    std::string lidar_topic;
    bool fisheye_model;
    int lidar_ring_count;
    std::pair<int, int> grid_size;
    int square_length;                        // in millimetres
    std::pair<int, int> board_dimension;      // in millimetres
    std::pair<int, int> cb_translation_error; // in millimetres
    cv::Mat cameramat;
    int distcoeff_num;
    cv::Mat distcoeff;
    std::pair<int, int> image_size;
} i_params;


//  define variable
image_transport::Publisher pub_img_dist;
#define PI 3.141592653589793238463
Rot_Trans rot_trans;
cv::Mat rvec(3, 1, cv::DataType<double>::type); // Initialization for pinhole and fisheye cameras
cv::Mat tvec(3, 1, cv::DataType<double>::type);


void image_projection(const sensor_msgs::Image::ConstPtr &img, const sensor_msgs::PointCloud2::ConstPtr &pc)
{
    cv_bridge::CvImagePtr cv_ptr;
    cv::Mat new_image_raw;
    pcl::PointCloud<pcl::PointXYZDR>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZDR>);

    cv_ptr = cv_bridge::toCvCopy(img, "bgr8");
    new_image_raw = cv_ptr->image;

    pcl::fromROSMsg(*pc, *cloud);

    
    if (cloud->size() < 1)
        return;

    std::vector<cv::Point3f> radar3dpoints;
    std::vector<cv::Point2f> image_points;
    for (pcl::PointCloud<pcl::PointXYZDR>::const_iterator it = cloud->begin(); it != cloud->end(); it++)
    {
        cv::Point3f radar3dpoint;
        if( it->x <10)
        {
            radar3dpoint.x = it->x;
            radar3dpoint.y = it->y;
            radar3dpoint.z = it->z;
            radar3dpoints.push_back(radar3dpoint);
        }

        
    }
    cv::projectPoints(radar3dpoints, rvec, tvec, i_params.cameramat, i_params.distcoeff, image_points);
    ROS_INFO("image_points");
    for (int i = 0; i < radar3dpoints.size(); i++)
    {
        if (image_points[i].y >= 0 and image_points[i].y < i_params.image_size.second and image_points[i].x >= 0 and image_points[i].y < i_params.image_size.first)
        {
        cv::circle(new_image_raw, image_points[i], 8, CV_RGB(255, 0, 0),-1);
        }
        // ROS_INFO("draw circle %d",i);
        // std::cout<<image_points[i]<<std::endl;
        // ROS_INFO("draw circle %f",i);
    }



    // Publish the image projection
    ros::Time time = ros::Time::now();
    cv_ptr->encoding = "bgr8";
    cv_ptr->header.stamp = time;
    cv_ptr->header.frame_id = "/traj_output";
    cv_ptr->image = new_image_raw;
    pub_img_dist.publish(cv_ptr->toImageMsg());
}

int main(int argc, char **argv)
{
    ROS_INFO("project2image");
    ros::init(argc, argv, "project2image");
    ros::NodeHandle n;

    // read param
    std::string pkg_loc = ros::package::getPath("reproject");
    std::ifstream infile(pkg_loc + "/cfg/reproject_params_naive.txt");
    infile >> i_params.camera_topic;
    infile >> i_params.lidar_topic;
    infile >> i_params.fisheye_model;
    infile >> i_params.lidar_ring_count;
    double camera_mat[9];
    for (int i = 0; i < 9; i++)
    {
        infile >> camera_mat[i];
    }

    cv::Mat(3, 3, CV_64F, &camera_mat).copyTo(i_params.cameramat);

    infile >> i_params.distcoeff_num;
    double dist_coeff[i_params.distcoeff_num];
    for (int i = 0; i < i_params.distcoeff_num; i++)
    {
        infile >> dist_coeff[i];
    }
    cv::Mat(1, i_params.distcoeff_num, CV_64F, &dist_coeff).copyTo(i_params.distcoeff);
    int  i_l, i_b;
    infile >> i_l;
    infile >> i_b;
    i_params.image_size = std::make_pair(i_l, i_b);


    infile>>rot_trans.e1;
    infile>>rot_trans.e2;
    infile>>rot_trans.e3;
    infile>>rot_trans.x;
    infile>>rot_trans.y;
    infile>>rot_trans.z;

    rvec.at<double>(0) = rot_trans.e1;
    rvec.at<double>(1) = rot_trans.e2;
    rvec.at<double>(2) = rot_trans.e3;
    tvec.at<double>(0) = rot_trans.x;
    tvec.at<double>(1) = rot_trans.y;
    tvec.at<double>(2) = rot_trans.z;



    // subscriber
    message_filters::Subscriber<sensor_msgs::Image> image_sub(n, i_params.camera_topic, 5);
    message_filters::Subscriber<sensor_msgs::PointCloud2> pcl_sub(n, i_params.lidar_topic, 5);
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::PointCloud2> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(5), image_sub, pcl_sub);
    sync.registerCallback(boost::bind(&image_projection, _1, _2));

    // publisher
    image_transport::ImageTransport it(n);
    pub_img_dist = it.advertise("image_projection", 20);

    ros::spin();
    return 0;
}